/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/motion_parameters.h
 * @Description  : 定义AGV运动参数的相关类和常量，包括速度、加速度、纠偏参数等， 
 *                 以及与AGV模型、曲线规划等相关的结构体和枚举类型。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:19:32
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "z_types.h"
#include "tools.h"
#include "rclcpp/rclcpp.hpp"
#include "struct_pid.h"
constexpr int LEVELS = 10;                  // 10 velocity levels
// Turn/Shift/Spin related macro
constexpr float TURN_MOTION_RATIO = 0.8f;   // Velocity ratio for turn 0.3 -> 0.8
constexpr float SHIFT_MOTION_RATIO = 0.5f;  // Velocity ratio for shift
constexpr float SPIN_MOTION_RATIO = 0.8f;   // 0.5f      // Velocity ratio for spin

constexpr float MIN_NORMAL_VEL = 0.1f;      // min. normal velocity: 0.1m/s
constexpr float MIN_SHIFT_VEL = 0.1f;       // min. shift velocity: 0.1m/s
constexpr float MIN_SPIN_VEL = 0.05f;       // min. spin velocity: 0.05m/s

constexpr float NORMAL_VEL_GATE = 0.08f;    // Vel gate for normal motion
constexpr float SHIFT_VEL_GATE = 0.08f;     // Vel gate for shift motion
constexpr float SPIN_VEL_GATE = 0.01f;      // Vel gate for spin-turn 0.03 -> 0.01

constexpr int PF_LASER_MAIN_GROUP = 4;
constexpr int PF_LASER_SUB_GROUP = 10;

// 车体模型枚举
enum class AgvModel {
    model_diff = 0,        // 差速
    model_steer = 1,       // 全方位舵轮
    model_single_steer = 2 // 单舵轮或叉车
};

enum class ParamSubType {
    LaserLineForward = 0,
    LaserLineBack,
    LaserCurve,
    LaserSide
};

// LineF-LineB-Curve-Sise:0~3
class ReviseParams
{
 public:
    ReviseParams() : modify_y_(1.0f), modify_angle_(1.0f) {}

    float modify_y_;
    float modify_angle_;
};

// 降速纠偏阈值参数
class RefrainVelParams
{
 public:
    RefrainVelParams() 
        : y_limit_bezier_(0.03f), angle_limit_bezier_(3.0f), 
        y_limit_line_(0.03f), angle_limit_line_(3.0f) {}

    float y_limit_bezier_;          // 左右偏差降速阈值(贝塞尔);(单位:m)
    float angle_limit_bezier_;      // 角度偏差降速阈值(贝塞尔);(单位:degree)
    float y_limit_line_;            // 左右偏差降速阈值(直线);(单位:m)
    float angle_limit_line_;        // 角度偏差降速阈值(直线);(单位:degree)
};

// 自旋时，低速停车阶段参数
class SpinParams
{
public:
    SpinParams()
        : small_slope_(0.05f), search_0_value_(0.08f), remain_angle_(10.0f) {}

    // 低速阶段减速度
    float small_slope_;
    // 低速阶段速度
    float search_0_value_;
    // 低速阶段还剩多大角度，开始使用低速速度
    float remain_angle_;
};

// 纠偏时，不同速度等级下的纠偏角加速度
class CorrectParams
{
 public:
    CorrectParams()
        : low_ang_acc_(8.0f), mid_ang_acc_(6.0f),
          high_ang_acc_(4.0f), super_high_ang_acc_(3.0f) {}

    // 低速阶段角加速度-0.5m/s 单位 degree/s
    float low_ang_acc_;
    // 中速阶段角加速度 -1m/s 单位 degree/s
    float mid_ang_acc_;
    // 高速阶段角加速度 -1.5m/s 单位 degree/s
    float high_ang_acc_;
    // 超高速速阶段角加速度-2m/s 单位 degree/s 以上都使用这个
    float super_high_ang_acc_;
};

class DefaultReviseParams
{
 public:
    DefaultReviseParams();
    ReviseParams laser_params_[PF_LASER_MAIN_GROUP][PF_LASER_SUB_GROUP]; // LineF-LineB-Curve-Sise:0~3
 private:
    void SetDefaultParam();
    void SetSpecialLaserFParam();
    void SetSpecialLaserBParam();
    void SetSpecialLaserCurveParam();
    void SetSpecialLaserSideParam();
};

// 记录载货标识或者开启
struct LoadFlags
{
    bool open;         // 是否开启区分有货无货速度开关
    bool current_load; // 当前载货标识

    LoadFlags() { Clear(); }

    void Clear() {
        open = false;
        current_load = false;
    }
};

// 空闲到启动状态发布结构
struct ReadyToRunDelay
{
    bool use;          // 是否开启
    int idle_time_ms;  // 空闲时间
    int delay_time_ms; // 延迟启动时间

    ReadyToRunDelay()
        : use(false), idle_time_ms(1000), delay_time_ms(3000) {}
};

// 曲线规划参数加载
struct CurveParamUnit
{
    CurveParamUnit() : vmax(2.0), jmax(1.0), amax(4.0) {}

    double vmax;
    double jmax;
    double amax;
};

// 曲线规划设定参数
struct CurveParamGroup
{
    CurveParamUnit line;
    CurveParamUnit bezier;
    CurveParamUnit spin;

    void OutLog(const std::string& name) {
        DEBUG_OUT(name << " group:"
                       << " line:vmax=" << line.vmax << ",jmax=" << line.jmax << ",amax=" << line.amax
                       << ",bezier:vmax=" << bezier.vmax << ",jmax=" << bezier.jmax << ",amax=" << bezier.amax
                       << ",spin:vmax=" << spin.vmax << ",jmax=" << spin.jmax << ",amax=" << spin.amax
        );
    }
};

struct CurveVelParams
{
    CurveParamGroup load;
    CurveParamGroup unload;
};

// 栈板识别末端返回路径控制点的距离系数
class SmoothParams
{
public:
    float cp1_k_; // 控制点1距离起点的距离系数
    float cp2_k_; // 控制点2距离终点的距离系数
    SmoothParams(): cp1_k_(1.0f), cp2_k_(1.0f) {}
};

//换向打舵前强制延迟(针对重载车)
struct SteerReadyDelayParams
{
    int delay_ms_;
    SteerReadyDelayParams(): delay_ms_(0) {}
};
//////////////////////////////////////////////////////////////////////////////
//   The interface of class "CMotionParm".
class MotionParm
{
public:
    // Default constructor
    MotionParm() = default;

    //init param
    static bool InitParam();

    // Update all parameters
    static void UpdateAllParam();

    // Auto update all parameters
    static void AutoUpdateAllParam();

    // 读取一次参数
    static bool EnableReadOnce();

    // 读取载货速度参数
    static bool ReadOpenLoadVelParam();

    // 读取距离参数
    static void SetDisParam();

    // 读取速度参数
    static void SetRefrainVelParam();

    // 读取自旋参数
    static void SetSpinParam();

    // 读取纠偏参数
    static void SetCorrectParam();

    // 读取曲线用户参数
    static void SetVelParam();

    // Set the current velocity level
    static void SetLevel(short level);

    // Get the normal velocity for the specified velocity level
    static float NormalVel(short level);

    // Get the turning velocity for the specified velocity level
    static float TurnVel(short level);

    // Get the shift velocity for the specified velocity level
    static float ShiftVel(short level);

    // Get the spin-turn velocity for the specified velocity level
    static float SpinVel(short level);

    // static void SetMaxVel(float fMaxVel);
    static void SetReviseFParam();

    // 设置贝塞尔反向参数
    static void SetReviseBParam();

    // 设置曲线反向参数
    static void SetReviseCurveParam();

    // 设置侧移反向参数
    static void SetReviseSideParam();

    // 读取agv模型
    static void ReadAgvModel();

    // 读取pid参数
    static void ReadPidParam();

    // 读取lqr参数
    static void ReadLqrParam();

    // 读取空闲到启动参数
    static void ReadToRunParam();

    // 读平滑参数
    static void ReadSmoothParam();
   
    // 读取舵参数
    static void ReadSteerParam();
public:
    static float up_acc_;               // Speeding-up acceleration (+) (m/s/s)
    static float down_acc_;             // Slowing-down acceleration (-) (m/s/s)
    static float down_low_acc_;         // Low acceleration (+) (m/s/s)
    static float max_vel_;              // Maximum linear velocity (m/s)
    static float min_adjust_radius_;    // Min. adjusting radius for path-following
    static float forward_stop_dis_;     // Auto Mode forward stop distance (m)
    static float backward_stop_dis_;    // Auto Mode backward stop distance (m)
    static float up_low_acc_;           // 起步时加速度
    static float transmit_vel_;         // 起步时目标速度 must == m_fSearch0Value ;fixbug
    static LoadFlags load_flags_;       // 是否开启区分有货无货速度

    static DefaultReviseParams revise_params_;          // 十级纠偏参数
    static RefrainVelParams refrain_params_;            // 降速纠偏阈值
    static int agv_model_;
    static SpinParams spin_params_;                     // 低速巡航阶段参数
    static CorrectParams correct_params_;               // 不同速度等级下的纠偏角加速度
    static PidGroup pid_params_;                        // pid纠偏参数
    static LqrParamReader lqr_params_;                  // lqr参数
    static ReadyToRunDelay read_to_run_delay_params_;   // 延迟启动参数
    static CurveVelParams curve_params_;                // 曲线规划参数
    static SmoothParams smooth_params_;  // 末端路径返回路段平滑系数
    static SteerReadyDelayParams steer_ready_params_;  //舵打舵前延迟

private:
    static float vel_step_;             // Velocity increment between 2 levels
    static short cur_level_;            // Current velocity level
    //    float m_fSearchVel;           // Station searching velocity (m/s)
    //    float m_fSearchDist;          // Station searching distance (m)
};

